#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#endif

#include <memory>
#include <chrono>
#include <string>
#include <boost/make_shared.hpp>
#include <vector>
#include "rclcpp/rclcpp.hpp"
//#include "std_msgs/msg/string.hpp"

#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/header.hpp"

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/registration/icp.h>
//#include <pcl/features/normal_3d.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
using namespace std;
//#include "unitree_lidar_sdk_pcl.h"
/**
 * @brief PCL Point Type
 */
struct PointType
{
  PCL_ADD_POINT4D
  PCL_ADD_INTENSITY
  std::uint16_t ring;
  float time;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PointType,
  (float, x, x)(float, y, y)(float, z, z)
  (float, intensity, intensity)
  (std::uint16_t, ring, ring)
  (float, time, time)
)

class MinimalSubscriber : public rclcpp::Node
{
  public:
    MinimalSubscriber()
    : Node("my_lidar")
    {
    
       using std::placeholders::_1;
        sub_unitree_lidar = this->create_subscription<sensor_msgs::msg::PointCloud2>("/unilidar/cloud", \
                10, std::bind(&MinimalSubscriber::topic_callback, this, std::placeholders::_1));
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_topic", 10);
    }

  private:
    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
    {
       int32_t size,i,j,k;
       pcl::PointCloud <PointType> lidar_cloud;
       pcl::PointCloud <pcl::PointXYZ> conver_cloud;
       pcl::PointXYZ tempPoint(0.0, 0.0, 0.0);
       pcl::fromROSMsg(*msg, lidar_cloud);
       size=lidar_cloud.points.size(); 
       RCLCPP_INFO(this->get_logger(), "I heard: '%d'", size);
       for(i=0;i<size;i++)
       {
        if((lidar_cloud.points[i].z<5.0)&&(lidar_cloud.points[i].y<0.0))
        {
            tempPoint.x=lidar_cloud.points[i].x;
            tempPoint.y=lidar_cloud.points[i].y;
            tempPoint.z=lidar_cloud.points[i].z;
            conver_cloud.push_back(tempPoint);
        }
       }
       

       // 假设已经有一个名为cloud的pcl::PointCloud<pcl::PointXYZ>对象
        // 和一个旋转角度（例如，以弧度为单位）
        float rotation_angle = M_PI *0.5; // 45度
         // 定义新的变换后的云
        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
      
      // 进行平移和角度变换 （x,y,z,anglex,angley,anglez)
      
      auto rotationMatrix=pcl::getTransformation(0.0, 0.0, 0.0, rotation_angle, 0.0, 3.1415926);
      pcl::transformPointCloud( conver_cloud,transformed_cloud ,rotationMatrix);
      //平移和角度变换end
      //累加多幅点云
      if(temp_count==4){temp_count=0;temp_send_flag=1;}
      temp_cloud[temp_count]=transformed_cloud;
      temp_count++;
      pcl::PointCloud<pcl::PointXYZ>::Ptr combined_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      float zero=0.0;
      size=0;
      if(temp_send_flag==1)
      {
        
        for(i=0;i<4;i++)
        {
          for(j=0;j<temp_cloud[i].points.size();j++)
          {
            if((temp_cloud[i].points[j].y>0.8)&&(temp_cloud[i].points[j].z<-0.20))
            { 
               combined_cloud->push_back(temp_cloud[i].points[j]);
              if((temp_cloud[i].points[j].y<1.2)&&(abs(temp_cloud[i].points[j].x<0.2)))
                    {
                      zero+=temp_cloud[i].points[j].z; size++;
                    }
            }
          }
        }
      
        if(size!=0) //地平面有数据
        {
          zero=zero/(float)size;   //得到地平面参考
           pcl::PointCloud<pcl::PointXYZ>::Ptr try1_cloud(new pcl::PointCloud<pcl::PointXYZ>); 
          pcl::PointCloud<pcl::PointXYZ>::Ptr try2_cloud(new pcl::PointCloud<pcl::PointXYZ>);
          size=combined_cloud->points.size();
           for(i=0;i<size;i++)  //再次去除比地面高20cm的点
            {
              if(combined_cloud->points[i].z<(zero+0.2))
                 try1_cloud->push_back(combined_cloud->points[i]);
            }
          pcl::VoxelGrid<pcl::PointXYZ> sor;
          sor.setInputCloud(try1_cloud);
          sor.setLeafSize(0.01f, 0.01f, 0.01f);  // 设置体素大小
          sor.filter(*try1_cloud);  // 执行滤波 
           // 创建K-D Tree对象
            pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
            // 向K-D Tree对象中填充点云
            kdtree.setInputCloud(try1_cloud);
              // 查找半径为0.5的球形区域内的所有点
          
            pcl::PointXYZ searchPoint(0.0, 0.0, 0.0);
            float radius = 0.15;
            std::vector<int> pointIdxRadiusSearch;
            std::vector<float> pointRadiusSquaredDistance;
            std::vector<int> pointstayit;
            size=try1_cloud->points.size();
            int pointcount=0;
           for(i=0;i<size;i++)  //对每个点规则判断
           {
             searchPoint.x=try1_cloud->points[i].x;
             searchPoint.y=try1_cloud->points[i].y-0.20;
             searchPoint.z=try1_cloud->points[i].z-0.05;
             radius=0.1;
             if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 4)
             {
              if(test_pingmian(try1_cloud,&pointIdxRadiusSearch,searchPoint.z)==0) //往后20cm是平面
                try2_cloud->push_back(searchPoint);
             }
           } 
          //size=mycloud->points.size();
            
            size=try2_cloud->points.size();
            
         ///测试数据开始
         pcl::PointCloud<pcl::PointXYZ>::Ptr tryaa_cloud(new pcl::PointCloud<pcl::PointXYZ>); 
          pcl::PointCloud<pcl::PointXYZ>::Ptr trybb_cloud(new pcl::PointCloud<pcl::PointXYZ>);
          pcl::PointCloud<pcl::PointXYZ>::Ptr trycc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
         float stepleft,stepright;
         stepleft=-0.1;
         int seat;
         for(j=0;j<14;j++)    ///向右搜寻
         { 
          stepleft+=0.1; stepright=stepleft+0.1;
           tryaa_cloud->clear();
          for(i=0;i<try1_cloud->points.size();i++)
           {
             
              if((try1_cloud->points[i].x<stepright)&&(try1_cloud->points[i].x>stepleft))
                 tryaa_cloud->push_back(try1_cloud->points[i]);
           }
          if(tryaa_cloud->points.size()>12)  //点太少了不判断
           {
            //------------------------------排序----------------------------
	          std::sort(tryaa_cloud->begin(), tryaa_cloud->end(),
		          [](pcl::PointXYZ a, pcl::PointXYZ b) {return a.y < b.y; });
            //---------------------------------------------------------------------
            
            seat=test_sort(tryaa_cloud,zero);
            if(seat!=0)
            {
                for(i=seat;i<tryaa_cloud->points.size();i++)
                {
                  if(abs(tryaa_cloud->points[seat].y-tryaa_cloud->points[i].y)<0.1)
                    trybb_cloud->push_back(tryaa_cloud->points[i]);
                }
            } else break;
           }
           
         }    
        //------------------------------------
        stepleft=0.0;
         
         for(j=0;j<14;j++)    ///向左搜寻
         { 
          stepleft-=0.1; stepright=stepleft+0.1;
           tryaa_cloud->clear();
          for(i=0;i<try1_cloud->points.size();i++)
           {
             
              if((try1_cloud->points[i].x<stepright)&&(try1_cloud->points[i].x>stepleft))
                 tryaa_cloud->push_back(try1_cloud->points[i]);
           }
          if(tryaa_cloud->points.size()>12)  //点太少了不判断
           {
            //------------------------------排序----------------------------
	          std::sort(tryaa_cloud->begin(), tryaa_cloud->end(),
		          [](pcl::PointXYZ a, pcl::PointXYZ b) {return a.y < b.y; });
            //---------------------------------------------------------------------
            
            seat=test_sort(tryaa_cloud,zero);
            if(seat!=0)
            {
                for(i=seat;i<tryaa_cloud->points.size();i++)
                {
                  if(abs(tryaa_cloud->points[seat].y-tryaa_cloud->points[i].y)<0.1)
                    trybb_cloud->push_back(tryaa_cloud->points[i]);
                }
            } else break;
           }
           
         }     
        //测试结束
        //去除离群点
        int liqun_index;
        // if(trybb_cloud->points.size()>20)
        // {
         // trybb_cloud->points.erase(trybb_cloud->points.begin()+5);
          // liqun_index=0;
          // while(liqun_index<1000)
          // {
          //   liqun_index=test_only_point_y(trybb_cloud,0);
          //    trybb_cloud->points.erase(trybb_cloud->points.begin()+liqun_index);
          // }
          // liqun_index=0;  
          // while(liqun_index<1000)
          // {
          //  liqun_index=test_only_point_y(trybb_cloud,1);
          //  trybb_cloud->points.erase(trybb_cloud->points.begin()+liqun_index);
          //}
       // }
            //发给rviz显示
            if(trybb_cloud->points.size()>5)
            pcl::io::savePCDFileASCII ("cloud2.pcd", *trybb_cloud);
            RCLCPP_INFO(this->get_logger(), "I send: '%d'  zero=%6.2f ", trybb_cloud->points.size(),zero);
            sensor_msgs::msg::PointCloud2 mymsg;
            pcl::toROSMsg(*trybb_cloud, mymsg);
            mymsg.header.stamp =now();
            mymsg.header.frame_id="map";
            publisher_->publish(mymsg);
       }
      }
    }
  int test_only_point_y(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,int sw)
  {
     size_t i;
     int flag=1000;
     switch(sw)
        {
          case 0:
            std::sort(input_cloud->begin(), input_cloud->end(),
                    [](pcl::PointXYZ a, pcl::PointXYZ b) {return a.y < b.y; });
            for ( i = 0; i < (input_cloud->points.size()/2); ++i) 
            {
              if (abs(input_cloud->points[i].y-input_cloud->points[i+1].y)>0.1) 
              {
                  //input_cloud->points.erase(input_cloud->points.begin() + i);
                  flag=i;break;
                  // --i; // 更新循环计数器，因为已经删除了一个元素
              }
            }
          break;
          case 1:
            std::sort(input_cloud->begin(), input_cloud->end(),
                    [](pcl::PointXYZ a, pcl::PointXYZ b) {return a.y > b.y; });
            for (i = 0; i < (input_cloud->points.size()/2); ++i) 
            {
              if (abs(input_cloud->points[i].y-input_cloud->points[i+1].y)>0.1) 
              {
                //  input_cloud->points.erase(input_cloud->points.begin() + i);
                  flag=i;break;
                  // --i; // 更新循环计数器，因为已经删除了一个元素
              }
            }
          break;
        }    
     return flag;         
  }
   //判断台阶 
  int test_sort(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,float ver)  
  {
      int i,size,flag=0;
      float plot;
      size=input_cloud->points.size();
      for(i=5;i<size-5;i++)
      {
       flag=0; 
       plot=(input_cloud->points[i-1].z+input_cloud->points[i-2].z+input_cloud->points[i-3].z+input_cloud->points[i-4].z)/4.0;
       if(abs(input_cloud->points[i-1].z-plot)>0.02)flag++; if(abs(input_cloud->points[i-2].z-plot)>0.02)flag++; 
       if(abs(input_cloud->points[i-3].z-plot)>0.02)flag++; if(abs(input_cloud->points[i-4].z-plot)>0.02)flag++; 
       if(flag==0)
       {
        if(input_cloud->points[i].z>(plot+0.05))return i-1;
        if(input_cloud->points[i+1].z>(plot+0.05))return i;
        if(input_cloud->points[i+2].z>(plot+0.05))return i+1;
        if(input_cloud->points[i+3].z>(plot+0.05))return i+2;
       }
      }
      return 0;
  }
  int test_pingmian(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,std::vector<int> *index,float ver)
    {
      int i,size,flag;
      float verf=0;
      size=index->size();
      flag=0;
      for(i=0;i<size;i++)
      {
        verf+=input_cloud->points[(*index)[i]].z;  
      }  
      verf=verf/(float)size;
      for(i=0;i<size;i++)
        if(abs(input_cloud->points[(*index)[i]].z-verf)>0.03)flag=1; //z值波动过大认为不是平面
     // if(flag==0)
     //   if((ver-0.02)<verf)flag=1;  
        return flag;
    } 

        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_unitree_lidar;
        pcl::PointCloud<pcl::PointXYZ> temp_cloud[5]; 
        int temp_count=0;
        int temp_send_flag=0;
      //  pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  
  rclcpp::shutdown();
  return 0;
}